/**
 * Copyright (c) 2021 Bosch Sensortec GmbH. All rights reserved.
 *
 * BSD-3-Clause
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 * 1. Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 *
 * 2. Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in the
 *    documentation and/or other materials provided with the distribution.
 *
 * 3. Neither the name of the copyright holder nor the names of its
 *    contributors may be used to endorse or promote products derived from
 *    this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
 * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
 * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
 * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT,
 * STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING
 * IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 *
 * @file	bsec2.cpp
 * @date	17 January 2023
 * @version	2.0.6
 *
 */

#include "bsec2.h"
#include <stdlib.h>
#include <string.h>
#include "mw_log_conf.h"
#include "stm32_timer.h"
#include "bme68x_function.h"

static uint8_t workBuffer[BSEC_MAX_WORKBUFFER_SIZE];
/* Stores the version of the BSEC algorithm */
extern bool BME_saveToEEPROMTemp;
bsec_version_t version;
bsec_library_return_t status;
bsec_bme_settings_t bmeConf;
bsecCallback newDataCallback01;
bsecOutputs outputs;
uint8_t bsecState[BSEC_MAX_STATE_BLOB_SIZE];
uint8_t opMode;
float extTempOffset;
uint32_t ovfCounter;
uint32_t lastMillis;

extern BME_Mode BME_mode;
extern uint8_t BME_gasPercentage;
extern uint8_t BME_gas_index_temp;
extern uint8_t BME_scanCycle;
extern uint8_t BME_scanCycle_temp;
extern uint8_t BME_iaqAccuracy;
extern uint8_t BME_runInStatus;
extern uint8_t BME_stabilizationStatus;
extern uint8_t BME_gasEstimate[4];
extern int16_t BME_gasTemperature;
extern uint16_t BME_staticIAQ;
extern uint16_t BME_rawHumidity;
extern uint16_t BME_gasHumidity;
extern uint16_t BME_IAQ;
extern uint16_t BME_gasPressure;
extern uint16_t BME_rawTemperature;
extern uint16_t BME_co2Equivalent;
extern uint16_t BME_VOCEquivalent;
extern uint32_t BME_rawGas;
extern uint32_t BME_gasResistance[BME_NUMBER_OF_PROF_MAX];
/**
 * @brief Constructor of Bsec2 class
 */
void Bsec2(void) {
    ovfCounter = 0;
    lastMillis = 0;
    status = BSEC_OK;
    extTempOffset = 0.0f;
    opMode = BME68X_SLEEP_MODE;
    //newDataCallback01 = NULL;

    memset(&version, 0, sizeof(version));
    memset(&bmeConf, 0, sizeof(bmeConf));
    memset(&outputs, 0, sizeof(outputs));
}

/**
 * @brief Function to initialize the sensor based on the Wire library
 */ 
bool begin(uint8_t i2cAddr, I2C_HandleTypeDef *i2c, bme68x_delay_us_fptr_t idleTask)
{
    begin01(i2cAddr, i2c, idleTask);

    if (checkStatus() == BME68X_ERROR)
        return false;

    return beginCommon();
}


/**
 * @brief Function to request/subscribe for desired virtual outputs with the supported sample rates
 */
bool updateSubscription(bsecSensor sensorList[], uint8_t nSensors, float sampleRate)
{
    bsec_sensor_configuration_t virtualSensors[BSEC_NUMBER_OUTPUTS], sensorSettings[BSEC_MAX_PHYSICAL_SENSOR];
    uint8_t nSensorSettings = BSEC_MAX_PHYSICAL_SENSOR;

    for (uint8_t i = 0; i < nSensors; i++)
    {
        virtualSensors[i].sensor_id = sensorList[i];
        virtualSensors[i].sample_rate = sampleRate;
    }

    /* Subscribe to library virtual sensors outputs */
    status = bsec_update_subscription(virtualSensors, nSensors, sensorSettings, &nSensorSettings);
    if (status != BSEC_OK)
        return false;

    return true;
}

/**
 * @brief Callback from the user to read data from the BME68X using parallel mode/forced mode, process and store outputs
 */
bool BME_run(void) {

    uint8_t nFieldsLeft = 0;
    bme68xData data;
    uint32_t currTimeMs = getTimeMs();
    int64_t nextCallMs = bmeConf.next_call / INT64_C(1000000);
    int64_t currTimeNs = 0;
    opMode = bmeConf.op_mode;
    if (currTimeMs >= nextCallMs)
    {
        currTimeNs = currTimeMs * INT64_C(1000000);
        /* Provides the information about the current sensor configuration that is
           necessary to fulfill the input requirements, eg: operation mode, timestamp
           at which the sensor data shall be fetched etc */
        status = bsec_sensor_control(currTimeNs, &bmeConf);
        if (status != BSEC_OK)
            return false;
        
        switch (bmeConf.op_mode)
        {
        case BME68X_FORCED_MODE:
            setBme68xConfigForced();
            break;
        case BME68X_PARALLEL_MODE:
            if (opMode != bmeConf.op_mode)
            {
                setBme68xConfigParallel();
            }
            break;

        case BME68X_SLEEP_MODE:
            if (opMode != bmeConf.op_mode)
            {
                setOpMode(BME68X_SLEEP_MODE);
                opMode = BME68X_SLEEP_MODE;
            }
            break;
        }

        if (checkStatus() == BME68X_ERROR){
            return false;
        }

        if (bmeConf.trigger_measurement && bmeConf.op_mode != BME68X_SLEEP_MODE)
        {
            if (fetchData())
            {
                do
                {
                    nFieldsLeft = getData01(&data);
                    /* check for valid gas data */
                    if (data.status & BME68X_GASM_VALID_MSK)
                    {
                        if (!processData(currTimeNs, data))
                        {
                            return false;
                        }
                    }
                } while (nFieldsLeft);
            }

        }

    }
    return true;
}

/**
 * @brief Function to get the state of the algorithm to save to non-volatile memory
 */
bool getState(uint8_t *state)
{
    uint32_t n_serialized_state = BSEC_MAX_STATE_BLOB_SIZE;

    status = bsec_get_state(0, state, BSEC_MAX_STATE_BLOB_SIZE, workBuffer, BSEC_MAX_WORKBUFFER_SIZE,
            &n_serialized_state);
    if (status != BSEC_OK)
        return false;
    return true;
}

/**
 * @brief Function to set the state of the algorithm from non-volatile memory
 */
bool setState(uint8_t *state)
{
    status = bsec_set_state(state, BSEC_MAX_STATE_BLOB_SIZE, workBuffer, BSEC_MAX_WORKBUFFER_SIZE);
    if (status != BSEC_OK)
        return false;

    memset(&bmeConf, 0, sizeof(bmeConf));

    return true;
}

/**
 * @brief Function to retrieve the current library configuration
 */
bool getConfig(uint8_t *config)
{
    uint32_t n_serialized_settings = 0;
    
    status = bsec_get_configuration(0, config, BSEC_MAX_PROPERTY_BLOB_SIZE, workBuffer, BSEC_MAX_WORKBUFFER_SIZE, &n_serialized_settings);
    if (status != BSEC_OK)
        return false;

    return true;
}

/**
 * @brief Function to set the configuration of the algorithm from memory
 */
bool setConfig(const uint8_t *config)
{
    status = bsec_set_configuration(config, BSEC_MAX_PROPERTY_BLOB_SIZE, workBuffer, BSEC_MAX_WORKBUFFER_SIZE);
    if (status != BSEC_OK)
        return false;

    memset(&bmeConf, 0, sizeof(bmeConf));

    return true;
}

/**
 * @brief Function to calculate an int64_t timestamp in milliseconds
 */
uint32_t getTimeMs(void)
{
    uint32_t timeMs = HAL_GetTick();
    /*
    if (lastMillis > timeMs)
    { 
        ovfCounter++;
    }
    */
    lastMillis = timeMs;
    return timeMs;// + (ovfCounter * INT64_C(0xFFFFFFFF));
}

/**
 * @brief Function to de-allocate the dynamically allocated memory
 */
void clearMemory(void)
{
}

/* Private functions */

/**
 * @brief Reads data from the BME68X sensor and process it
 */
bool processData(int64_t currTimeNs, const bme68xData data)
{
    bsec_input_t inputs[BSEC_MAX_PHYSICAL_SENSOR]; /* Temp, Pres, Hum & Gas */
    uint8_t nInputs = 0;
    /* Checks all the required sensor inputs, required for the BSEC library for the requested outputs */
    if (BSEC_CHECK_INPUT(bmeConf.process_data, BSEC_INPUT_HEATSOURCE))
    {
        inputs[nInputs].sensor_id = BSEC_INPUT_HEATSOURCE;
        inputs[nInputs].signal = extTempOffset;
        inputs[nInputs].time_stamp = currTimeNs;
        nInputs++;
    }
    if (BSEC_CHECK_INPUT(bmeConf.process_data, BSEC_INPUT_TEMPERATURE))
    {
#ifdef BME68X_USE_FPU
        inputs[nInputs].signal = data.temperature;
#else
        inputs[nInputs].signal = data.temperature / 100.0f;
#endif
        inputs[nInputs].sensor_id = BSEC_INPUT_TEMPERATURE;
        inputs[nInputs].time_stamp = currTimeNs;
        nInputs++;
    }
    if (BSEC_CHECK_INPUT(bmeConf.process_data, BSEC_INPUT_HUMIDITY))
    {
#ifdef BME68X_USE_FPU
        inputs[nInputs].signal = data.humidity;
#else
        inputs[nInputs].signal = data.humidity / 1000.0f;
#endif
        inputs[nInputs].sensor_id = BSEC_INPUT_HUMIDITY;
        inputs[nInputs].time_stamp = currTimeNs;
        nInputs++;
    }
    if (BSEC_CHECK_INPUT(bmeConf.process_data, BSEC_INPUT_PRESSURE))
    {
        inputs[nInputs].sensor_id = BSEC_INPUT_PRESSURE;
        inputs[nInputs].signal = data.pressure;
        inputs[nInputs].time_stamp = currTimeNs;
        nInputs++;
    }
    if (BSEC_CHECK_INPUT(bmeConf.process_data, BSEC_INPUT_GASRESISTOR) &&
            (data.status & BME68X_GASM_VALID_MSK))
    {
        inputs[nInputs].sensor_id = BSEC_INPUT_GASRESISTOR;
        inputs[nInputs].signal = data.gas_resistance;
        inputs[nInputs].time_stamp = currTimeNs;
        nInputs++;
    }
    if (BSEC_CHECK_INPUT(bmeConf.process_data, BSEC_INPUT_PROFILE_PART) &&
            (data.status & BME68X_GASM_VALID_MSK))
    {
        inputs[nInputs].sensor_id = BSEC_INPUT_PROFILE_PART;
        inputs[nInputs].signal = (opMode == BME68X_FORCED_MODE) ? 0 : data.gas_index;
        inputs[nInputs].time_stamp = currTimeNs;
        nInputs++;
    }

    if (nInputs > 0)
    {

        outputs.nOutputs = BSEC_NUMBER_OUTPUTS;
        memset(outputs.output, 0, sizeof(outputs.output));

        /* Processing of the input signals and returning of output samples is performed by bsec_do_steps() */
        status = bsec_do_steps(inputs, nInputs, outputs.output, &outputs.nOutputs);

        if (status != BSEC_OK)
            return false;

        if(newDataCallback01){
            newDataCallback(data, outputs);
        }
    }
    return true;
}

/**
 * @brief Common code for the begin function
 */
bool beginCommon(){
    status = bsec_init();
    if (status != BSEC_OK)
        return false;

    status = bsec_get_version(&version);
    if (status != BSEC_OK)
        return false;

    memset(&bmeConf, 0, sizeof(bmeConf));
    memset(&outputs, 0, sizeof(outputs));

    return true;
}

/**
 * @brief Set the BME68X sensor configuration to forced mode
 */
void setBme68xConfigForced(void) {
    /* Set the filter, odr, temperature, pressure and humidity settings */
    setTPH(bmeConf.temperature_oversampling, bmeConf.pressure_oversampling, bmeConf.humidity_oversampling);

    if (checkStatus() == BME68X_ERROR)
        return;

    setHeaterProf01(bmeConf.heater_temperature, bmeConf.heater_duration);

    if (checkStatus() == BME68X_ERROR)
        return;

    setOpMode(BME68X_FORCED_MODE);
    if (checkStatus() == BME68X_ERROR)
        return;

    opMode = BME68X_FORCED_MODE;
}

/**
 * @brief Set the BME68X sensor configuration to parallel mode
 */
void setBme68xConfigParallel(void) {
    uint16_t sharedHeaterDur = 0;

    /* Set the filter, odr, temperature, pressure and humidity settings */
    setTPH(bmeConf.temperature_oversampling, bmeConf.pressure_oversampling, bmeConf.humidity_oversampling);

    if (checkStatus() == BME68X_ERROR)
        return;

    sharedHeaterDur = BSEC_TOTAL_HEAT_DUR - (getMeasDur(BME68X_PARALLEL_MODE) / INT64_C(1000));
    setHeaterProf03(bmeConf.heater_temperature_profile, bmeConf.heater_duration_profile, sharedHeaterDur,
            bmeConf.heater_profile_len);

    if (checkStatus() == BME68X_ERROR)
        return;

    setOpMode(BME68X_PARALLEL_MODE);

    if (checkStatus() == BME68X_ERROR)
        return;

    opMode = BME68X_PARALLEL_MODE;
}

const bsecOutputs* getOutputs(void){
    if (outputs.nOutputs)
        return &outputs;
    return NULL;
}

void attachCallback(bsecCallback callback) {
    newDataCallback01 = callback;
}

bsecData getData(bsecSensor id) {
    bsecData emp;
    memset(&emp, 0, sizeof(emp));
    for (uint8_t i = 0; i < outputs.nOutputs; i++)
        if (id == outputs.output[i].sensor_id)
            return outputs.output[i];
    return emp;
}

void setTemperatureOffset(float tempOffset) {
    extTempOffset = tempOffset;
}

void newDataCallback(const bme68xData data, const bsecOutputs outputs) {

    if (!outputs.nOutputs) {
        return;
    }
		
	char foo[20];
    for (uint8_t i = 0; i < outputs.nOutputs; i++) {
        const bsecData output = outputs.output[i];
        switch (output.sensor_id) {
            case BSEC_OUTPUT_IAQ:
                BME_IAQ = (int)(output.signal * 100);
                BME_iaqAccuracy = output.accuracy;
                printf("IAQ: %d %d\r\n",
                    (int)(output.signal * 100), output.accuracy);
                break;
            case BSEC_OUTPUT_STATIC_IAQ:
                BME_staticIAQ = (int)(output.signal * 100);
                printf("BME_staticIAQ: %d\r\n",
                    (int)(output.signal * 100));
                break;
            case BSEC_OUTPUT_CO2_EQUIVALENT:
                BME_co2Equivalent = (int)(output.signal);
                printf("BME_co2Equivalent: %d\r\n",
                    (int)(output.signal));
                break;
            case BSEC_OUTPUT_BREATH_VOC_EQUIVALENT:
                BME_VOCEquivalent = (int)(output.signal * 10);
                printf("BME_VOCEquivalent: %d\r\n",
                    (int)(output.signal * 10));
                break;
            case BSEC_OUTPUT_RAW_TEMPERATURE:
                BME_rawTemperature = (int)(output.signal * 100);
                printf("BME_rawTemperature: %d\r\n",
                    (int)(output.signal * 100));
                break;
            case BSEC_OUTPUT_RAW_HUMIDITY:
                BME_rawHumidity = (int)(output.signal * 100);
                printf("BME_rawHumidity: %d\r\n",
                    (int)(output.signal * 100));
                break;
            case BSEC_OUTPUT_RAW_GAS:
                BME_rawGas = (int)(output.signal);
                printf("BME_rawGas: %d\r\n",
                    (int)(output.signal));
                if (BME_gas_index_temp == 0) {
                    BME_gasResistance[9] =
                        (int)(output.signal);
                    printf("BME_rawGas_0: %d %d\r\n", 9,
                        (int)(output.signal));
                }
                else {
                    BME_gasResistance[BME_gas_index_temp - 1] =
                        (int)(output.signal);
                    printf("BME_rawGas: %d %d\r\n",
                        BME_gas_index_temp - 1, (int)(output.signal));
                }
                break;
            case BSEC_OUTPUT_STABILIZATION_STATUS:
                BME_stabilizationStatus = (int)(output.signal);
                printf("BME_stabilizationStatus: %d\r\n",
                    (int)(output.signal));
                break;
            case BSEC_OUTPUT_RUN_IN_STATUS:
                BME_runInStatus = (int)(output.signal);
                printf("BME_runInStatus: %d\r\n",
                    (int)(output.signal));
                break;
            case BSEC_OUTPUT_SENSOR_HEAT_COMPENSATED_TEMPERATURE:
                BME_gasTemperature = (int)(output.signal * 100);
                printf("BME_gasTemperature: %d\r\n",
                    (int)(output.signal * 100));
                break;
            case BSEC_OUTPUT_SENSOR_HEAT_COMPENSATED_HUMIDITY:
                BME_gasHumidity = (int)(output.signal * 100);
                printf("BME_gasHumidity: %d\r\n",
                    (int)(output.signal * 100));
                break;
            case BSEC_OUTPUT_RAW_PRESSURE:
                BME_gasPressure = (int)(output.signal) / 100;
                printf("BME_gasPressure: %d\r\n", 
                    BME_gasPressure);
                break;
            case BSEC_OUTPUT_GAS_PERCENTAGE:
                BME_gasPercentage = (int)output.signal;
                printf("BME_gasPercentage: %d\r\n", 
                    (int)output.signal);
                break;
			case BSEC_OUTPUT_RAW_GAS_INDEX:
                BME_gas_index_temp = (int)output.signal;
                printf("BME_gas_index_temp: %d\r\n", 
                    (int)output.signal);
                break;
			case BSEC_OUTPUT_GAS_ESTIMATE_1:
                BME_gasEstimate[0] = (uint8_t)(output.signal * 100);
                printf("BME_gasEstimate[0]: %d\r\n", 
                    (uint8_t)(output.signal * 100));
                break;
			case BSEC_OUTPUT_GAS_ESTIMATE_2:
                BME_gasEstimate[1] = (uint8_t)(output.signal * 100);
                printf("BME_gasEstimate[1]: %d\r\n", 
                    (uint8_t)(output.signal * 100));
                break;
			case BSEC_OUTPUT_GAS_ESTIMATE_3:
                BME_gasEstimate[2] = (uint8_t)(output.signal * 100);
                printf("BME_gasEstimate[2]: %d\r\n", 
                    (uint8_t)(output.signal * 100));
                break;
			case BSEC_OUTPUT_GAS_ESTIMATE_4:
                BME_gasEstimate[3] = (uint8_t)(output.signal * 100);
                printf("BME_gasEstimate[3]: %d\r\n", 
                    (uint8_t)(output.signal * 100));
                break;
            default:
                break;
        }
    }
}